
#ifndef __DRIVER_PID_CONTROLLER_H
#define __DRIVER_PID_CONTROLLER_H


#include "stdint.h"
#include "arm_math.h"

typedef enum
{
	PI_SERISE=0,
	PI_PARALLEL,
	PI_ROBUST_PARALLEL,
	PID_PARALLEL
	
}ENUM_PID_MODE;

typedef struct 
{	
	float32_t kp;
	float32_t ki;
	float32_t kd;
	
	float32_t ui;  //the integrator start value
	float32_t historyerro;
	
	float32_t spearate;
	float32_t robustpoint;
	
	float32_t refvalue;
	float32_t fbackvaule;
	float32_t ffwdvalue;

	float32_t outmax;
	float32_t outmin;
	
	float32_t outvalue;

}STR_PID_HDL;



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
static __INLINE void pi_run_serise(STR_PID_HDL* hdl)
{

	float32_t erro;
	float32_t up;
	float32_t ui;
	float32_t out;
	
	erro = hdl->refvalue - hdl->fbackvaule;
	
	up = erro * hdl->kp;
	
	ui = hdl->ui + hdl->ki * up;
	
	if(ui > hdl->outmax)       ui = hdl->outmax;
	else if(ui < hdl->outmin)  ui = hdl->outmin;
	
	
	out = up + ui + hdl->ffwdvalue;
	
	if(out > hdl->outmax)       out = hdl->outmax;
	else if(out < hdl->outmin)  out = hdl->outmin;

	
	hdl->ui = ui;
	hdl->outvalue = out;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
static __INLINE void pi_run_parallel(STR_PID_HDL* hdl)
{
	
	float32_t erro;
	float32_t up;
	float32_t ui;
	float32_t out;

	erro = hdl->refvalue - hdl->fbackvaule;
	
	up = erro * hdl->kp;
	
	ui = hdl->ui + hdl->ki * erro;
	
	if(ui > hdl->outmax)       ui = hdl->outmax;
	else if(ui < hdl->outmin)  ui = hdl->outmin;
	
	
	out = up + ui + hdl->ffwdvalue;
	
	if(out > hdl->outmax)       out = hdl->outmax;
	else if(out < hdl->outmin)  out = hdl->outmin;

	
	hdl->ui = ui;
	hdl->outvalue = out;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

static __INLINE void pi_robust_run_parallel(STR_PID_HDL* hdl)
{
	
	float32_t erro;
	float32_t erro_thre;
	float32_t up;
	float32_t ui;
	float32_t out;

	erro = hdl->refvalue - hdl->fbackvaule;
	erro_thre = hdl->robustpoint;
	
	// bangbang ctrl mode 
	if(erro > erro_thre)
	{
		out = hdl->outmax;
		ui = hdl->outmax * 0.2f;
	}
	else if(erro < -erro_thre)
	{
		out = hdl->outmin;
		ui = hdl->outmin * 0.2f;
	}	
	
	// pi ctrl mode
	else
	{
		up = erro * hdl->kp;
		
		ui = hdl->ui + hdl->ki * erro;
		
		if(ui > hdl->outmax)       ui = hdl->outmax;
		else if(ui < hdl->outmin)  ui = hdl->outmin;
		
		out = up + ui + hdl->ffwdvalue;
	}
	
	if(out > hdl->outmax)       out = hdl->outmax;
	else if(out < hdl->outmin)  out = hdl->outmin;
	
	hdl->ui = ui;
	hdl->outvalue = out;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
static __INLINE void pid_run_parallel(STR_PID_HDL* hdl)
{
	float32_t erro;
	float32_t erro2;
	float32_t ks;
	float32_t up;
	float32_t ui;
	float32_t ud;
	float32_t out;

	erro = hdl->refvalue - hdl->fbackvaule;
	
	erro2 = erro - hdl->historyerro;
	
	up = erro * hdl->kp;
	
	if((erro > hdl->spearate) || (erro < (-hdl->spearate)))
	{
		ks = 0;
	}
	else
	{
		ks = 1;
	}
	
	ui = (hdl->ui + hdl->ki * erro) * ks;
	
	if(ui > hdl->outmax)       ui = hdl->outmax;
	else if(ui < hdl->outmin)  ui = hdl->outmin;
	
	ud = hdl->kd * erro2;
	
	out = up + ui + ud + hdl->ffwdvalue;
	
	if(out > hdl->outmax)       out = hdl->outmax;
	else if(out < hdl->outmin)  out = hdl->outmin;

	hdl->ui = ui;
	hdl->historyerro = erro;
	hdl->outvalue = out;
}

extern STR_PID_HDL linespeedHdl[2];
extern STR_PID_HDL angularspeedHdl;


extern void pid_handle_init(STR_PID_HDL* hdl);

#endif

